#include "alice3D.h"

void alice3D::D3drawText(float x0,float y0,float z0,char* T,int color)
{
	float Ax,Ay;
	point(x0,y0,z0);
	projectOrtho(Ax,Ay);
        text(X0+(int)(Ax*size),Y0-(int)(Ay*size),T,color);        
}

void alice3D::D3drawLine(float x0,float y0,float z0,float x1,float y1,float z1,int color)
{
	float Ax,Ay,Bx,By;
	point(x0,y0,z0);
	projectOrtho(Ax,Ay);
	point(x1,y1,z1);
 	projectOrtho(Bx,By);
        line(X0+(int)(Ax*size),Y0-(int)(Ay*size),X0+(int)(Bx*size),Y0-(int)(By*size),color);        

}

void alice3D::D3drawAxes()
{

    D3drawLine(0,0,0,1.3,0,0,getRGB(60,0,0));
    D3drawLine(0,0,0,0,1.3,0,getRGB(0,60,0));
    D3drawLine(0,0,0,0,0,1.3,getRGB(0,0,60));

    D3drawText(1,0,0,"X\n",getRGB(60,0,0));
    D3drawText(0,1,0,"Y\n",getRGB(0,60,0));
    D3drawText(0,0,1,"Z\n",getRGB(0,0,60));

    D3drawCube(); 
}

void alice3D::D3drawCompass()
{

    D3drawLine(0,0,0,1.5,0,0,getRGB(60,0,0));
    D3drawLine(0,0,0,0,0,-1.5,getRGB(0,0,60));

    D3drawText(1.7,0,0,"N\n",getRGB(60,0,0));
    D3drawText(0,0,-1.7,"G\n",getRGB(0,0,60));

    D3drawCube(); 
    

}


void alice3D::D3drawPolyline(float p[][AFINE_DIM],int v)
{
//NO FUNCIONA
	int i;
	Serial.print(p[1][1]);
	for (i=0;i<v-1;i++){
		D3drawLine(p[i][0],p[i][1],p[i][2],p[i+1][0],p[i+1][1],p[i+1][2],getRGB(40,40,40));
	}
}

void alice3D::D3drawCube()
{
    int c=getRGB(30,30,30);
    D3drawLine(1,0,0,1,1,0,c);
    D3drawLine(1,0,0,1,0,1,c);
    D3drawLine(1,1,0,1,1,1,c);

    D3drawLine(0,1,0,1,1,0,c);
    D3drawLine(0,1,0,0,1,1,c);
    D3drawLine(0,1,1,1,1,1,c);

    D3drawLine(0,0,1,1,0,1,c);
    D3drawLine(0,0,1,0,1,1,c);
    D3drawLine(1,0,1,1,1,1,c);


}
